#include "testasservissement.h"

#include <QTest>
/*
Automatic clinic tests using a robot arm.
Copyright (C) 2010  CIC-CPC, CHU Gabriel Montpied
All other trademarks, logos and copyrights are the property
of their respective owners.

This file is part of cpc-painbot.

cpc-painbot is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

cpc-painbot is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with cpc-painbot.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <QSignalMapper>

#include "bras.h"
#include "capteur/axom4013.h"
#include "indicateurprocess.h"
#include "asservissement/asservissement.h"
#include "asservissement/regulateurdistance.h"

void TestAsservissement::initTestCase()
{
    bras = new Bras(this);
    indicateur = new IndicateurProcess(this);
    bras->setPort("COM6");
    indicateur->setPort("COM1");
    indicateur->setAdresse(1);
    indicateur->connecter();
    bras->connecter();

    // Attendre que le bras réponde.
    while ( bras->etat() != Bras::EtatNonPret )
        QTest::qWait(250);

    bras->initialiser();

    // Attendre que le bras soit initialisé.
    while ( bras->etat() != Bras::EtatPret )
        QTest::qWait(10000);

    // Placer le bras à -50 mm en hauteur (marge de manoeuvre)
    bras->deplacer(0, 0, 50, 0, 50);

    while ( bras->etat() != Bras::EtatPret )
        QTest::qWait(1000);
}

void TestAsservissement::testAsservissement()
{
    Capteur *capteur = new capteur::Axom4013(this);
    Asservissement *asservissement = new Asservissement(this);
    RegulateurDistance *reg_distance = new RegulateurDistance(this);

    // Consigne de 40mm en hauteur
    reg_distance->setConsigne(qreal(40));
    indicateur->setCapteur(capteur);

    asservissement->setRegulateur(reg_distance);
    asservissement->setCapteur("distance", capteur);
    asservissement->setAsservi("bras", bras, SIGNAL(positionChangee(PositionBras)));

    indicateur->demarrer();

    // Forcer une première régulation
    asservissement->reguler();

    QTest::qWait(60000);
    indicateur->arreter();
    QFAIL("Pas encore implémenté");
}
